#include "TranslatingObject.h"

TranslatingObject::TranslatingObject(	TextureManager * textureManager,
										Material * material,
										float textureRotation,
										Vector2D * vertices,
										int numVertices,
										float mass,
										Vector2D * path,
										int numPathPoints,
										float speed)
: Object( textureManager, material, textureRotation, Vector2D(0,0),Vector2D(0,0),
		  Vector2D(0,0), vertices, numVertices,0.0,0.0,0.0,mass,false)
{
	collideWhenGrabbed = true;
	this->path = path;
	this->numPathPoints = numPathPoints;
	position = path[0];
	currentPathPoint = 0;
	this->speed = speed;
	circular = (path[0] == path[numPathPoints - 1]);
	forward = true;
}

TranslatingObject::~TranslatingObject()
{
	if(path)
		delete(path);
	path = NULL;
	numPathPoints = 0;
}

void TranslatingObject::Think(double elapsedTime)
{
	if(PreThinkEntryPoint(elapsedTime))
		return;
	
	if(isPinned)
	{
		velocity = Vector2D(0,0);
		return;
	}
	float currentSpeed = speed;
	float dist = position.dist(path[currentPathPoint]);
	int nextPathPoint = currentPathPoint;
	if(dist <= speed * elapsedTime)
	{
		currentSpeed = dist / elapsedTime;
		
		if(circular)
			nextPathPoint = (currentPathPoint+1)%numPathPoints;
		else
		{
			if(forward)
				nextPathPoint++;
			else
				nextPathPoint--;
				
			if(nextPathPoint >= numPathPoints || nextPathPoint < 0)
			{
				if(forward)
				{
					nextPathPoint = numPathPoints - 2;
					forward = false;
				}
				else
				{
					nextPathPoint = 1;
					forward = true;
				}
			}
		}
	}
	Vector2D direction = path[currentPathPoint] - position;
	if(direction.Magnitude() == 0.0)
	{
		velocity = Vector2D(0,0);
	}
	else
	{
		direction = !direction;
		
		velocity = direction * currentSpeed;
		position = position + (velocity * elapsedTime);
	}
		
	ThinkEntryPoint(elapsedTime);
	
	translationalForce = Vector2D(0,0);
	torque = 0.0;
	currentPathPoint = nextPathPoint;
	
	if(hitGround)
	{
		onGroundFilter = 0;
		isOnGround = true;
	}
	else if(isOnGround)
		onGroundFilter++;
	
	if(onGroundFilter > 5)
		isOnGround = false;
		
	hitGround = false;
	isOnRightWall = false;
	isOnLeftWall = false;
	lastElapsedTime = elapsedTime;
}
void TranslatingObject::DrawEntryPoint()
{
	glPushMatrix();
	glLoadIdentity();
	glColor3f(1,1,1);
	glBegin(GL_LINES);
	for(int i = 0; i < numPathPoints - 1; i++)
	{
		glVertex3f(path[i].x, path[i].y, -0.1);
		glVertex3f(path[i+1].x, path[i+1].y, -0.1);
	}
	glEnd();
	glPopMatrix();
}